﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace PID.Demo
{
    public class PIDController
    {
        private double _kp, _ki, _kd, alpha = 0.2d;
        private double _integral, _preError, _setPoint, _lastActualValue = 0d;

        public PIDController(double kp, double ki, double kd)
        {
            _kp = kp;
            _ki = ki;
            _kd = kd;
        }
        public void InitDefault()
        {
            _integral = 0d;
            _lastActualValue = 0d;
            _setPoint = 0d;
            _preError = 0d;

        }
        public double MaxLim { get; set; }
        public double MinLim { get; set; }
        public double P
        {
            get
            {
                return _kp;
            }
            set
            {
                _kp = value;
            }
        }
        public double I
        {
            get
            {
                return _ki;
            }
            set
            {
                _ki = value;
            }
        }
        public double D
        {
            get
            {
                return _kd;
            }
            set
            {
                _kd = value;
            }
        }
        /// <summary>
        /// 是否开启死区
        /// </summary>
        public bool OpenDeadZone { get; set; }
        public double DeadZoneMaxLim { get; set; }
        public double DeadZoneMinLim { get; set; }
        /// <summary>
        /// 积分分离系数(10到100)
        /// 1.较小的IntegralSeparationFactor：如果你希望积分项对小误差更加敏感，可以选择较小的IntegralSeparationFactor。
        ///     这样可以使积分项在较小的误差范围内开始累积，从而更早地对系统进行修正。适用于对精度要求较高的系统。
        /// 2.较大的IntegralSeparationFactor：如果你希望积分项对小误差不敏感，可以选择较大的IntegralSeparationFactor。
        ///     这样可以避免死区内的误差对积分项的影响，减少积分项的累积。适用于系统存在较大噪声或对小误差不敏感的情况。
        /// </summary>
        public double IntegralSeparationFactor { get; set; }
        public bool OpenInput { get; set; }
        public double InputUpperLimit { get; set; }
        public double InputLowerLimit { get; set; }
        public double MaxIntegral { get; set; }
        public double LastActualValue
        {
            get { return _lastActualValue; }
            set
            {
                _lastActualValue = value;
            }
        }
        public double UnfilteredActualValue { get; set; }
        /// <summary>
        ///  是否开启抗饱和
        /// </summary>
        public bool OpenSaturation { get; set; }
        /// <summary>
        /// 是否开启微分抗干扰
        /// </summary>
        public bool OpenInterfere { get; set; }
        public double Alpha
        {
            get
            {
                return alpha;
            }
            set {
                if (value > 1 || value < 0) return;
                alpha = value;
            }
        }
        /// <summary>
        /// 是否开启积分分离
        /// </summary>
        public bool OpenIntegral { get; set; }
        public double Calculate(double setPoint, double actualPoint)
        {
            _setPoint = setPoint;
            // 输入抗饱和
            if (OpenInput)
                actualPoint = InputLimit(actualPoint);

            double error = setPoint - actualPoint;
            double errorAbs = Math.Abs(error);
            // 死区不调整
            if (OpenDeadZone && (error < DeadZoneMaxLim && error > DeadZoneMinLim))
                return _lastActualValue;

            _integral += error;
            // 积分
            double iOut = 0;
            // 积分分离
            if (OpenIntegral)
            {
                bool applyIntegralSeparation = errorAbs > IntegralSeparationFactor;
                // 积分分离
                if (applyIntegralSeparation)
                {
                    iOut = 0;
                }
            }
            else
            {
                iOut = _ki * _integral;

            }

            // 比例
            double pOut = _kp * error;
            double derivative = error - _preError;
            if (OpenInterfere)
                // 微分项对输入信号的噪声非常敏感，因此通常需要对微分项进行低通滤波，作用：抗干扰
                derivative = alpha * derivative + (1 - alpha) * (error - _preError);

            _preError = error;
            // 微分
            double dOut = _kd * derivative;

            // 计算
            var actualValue = pOut + iOut + dOut;
            UnfilteredActualValue = actualValue;
            // 饱和函数限制输出值
            if (OpenSaturation)
                actualValue = Saturation(actualValue);

            _lastActualValue = actualValue;
            return actualValue;
        }
        private double InputLimit(double actualValue)
        {
            if (actualValue > InputUpperLimit)
                actualValue = InputUpperLimit;
            if (actualValue < InputLowerLimit)
                actualValue = InputLowerLimit;

            return actualValue;
        }
        private double Saturation(double actualValue)
        {
            if (actualValue > MaxLim)
                actualValue = MaxLim;
            if (actualValue < MinLim)
                actualValue = MinLim;

            return actualValue;
        }
    }
}
